package ru.spbau.lifeincube.math;

public class Matrix3x3 {
	private double[] data;

	public Matrix3x3() {
		data = new double[9];
		data[0] = 1;
		data[3] = 1;
		data[6] = 1;
	}

	public Matrix3x3(Matrix4x4 mat) {
		this();
		for (int i = 0; i < 3; i++){
			for (int j = 0; j < 3; j++){
				data[i*3 + j] = mat.get(i, j);
			}
		}
	}

	public Matrix3x3(double[] a) {
		data = a;
	}

	public Matrix3x3(Quaternion quaternion) {
		this();

		double x = quaternion.getX();
		double y = quaternion.getY();
		double z = quaternion.getZ();
		double w = quaternion.getW();

		data[0] = 1.0 - 2.0 * y * y - 2.0 * z * z;
		data[1] = 2.0 * x * y + 2.0 * w * z;
		data[2] = 2.0 * x * z - 2.0 * w * y;

		data[3] = 2.0 * x * y - 2.0 * w * z;
		data[4] = 1.0 - 2.0 * x * x - 2.0 * z * z;
		data[5] = 2.0 * y * z + 2.0 * w * x;

		data[6] = 2.0 * x * z + 2.0 * w * y;
		data[7] = 2.0 * y * z - 2.0 * w * x;
		data[8] = 1.0 - 2.0 * x * x - 2.0 * y * y;
	}

	public void mult(Matrix3x3 mat) {
		int i;
		int k;

		double[] temp = new double[9];

		int row;
		int column;
		for (i = 0; i < 9; i++)
		{
			temp[i] = 0.0;

			row = i % 3;
			column = (i / 3) * 3;

			for (k = 0; k < 3; k++)
			{
				temp[i] += data[row + k * 3] * mat.data[k + column];
			}
		}

		for (i = 0; i < 9; i++)
		{
			data[i] = temp[i];
		}
	}

	public void substract(Matrix3x3 mat) {
		for (int i = 0; i < 3; i ++) {
			for (int j = 0; j < 3; j++) {
				set(i, j, get(i, j) - mat.get(i, j));
			}
		}
	}

	public void add(Matrix3x3 mat) {
		for (int i = 0; i < 3; i ++) {
			for (int j = 0; j < 3; j++) {
				set(i, j, get(i, j) + mat.get(i, j));
			}
		}
	}

	public void multScalar(double scalar) {
		for (int i = 0; i < 3; i ++) {
			for (int j = 0; j < 3; j++) {
				set(i, j, get(i, j) *scalar);
			}
		}
	}

	public Matrix3x3 getTransposed() {
		Matrix3x3 temp = new Matrix3x3();
		for (int i = 0; i < 3; i ++) {
			for (int j = 0; j < 3; j++) {
				temp.set(i,j, get(j, i));
			}
		}
		return temp;
	}

	public double get(int i, int j) {
		return data[3*i + j];
	}

	public void set(int i, int j, double val) {
		data[3*i + j] = val;
	}

	public double trace() {
		return data[0] + data[4] + data[8];
	}
}
